/**
 * @file      HMC5883L.c
 * @brief     HMC5883L三轴磁力计传感器驱动实现
 * @version   1.0
 * @date      2025-01-22
 */

#include "HMC5883L.h"
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <math.h>
#include "ti_msp_dl_config.h"
#include "../../CLOCK/clock.h"

/*************************************************************************************************/
/* 私有定义 */
/*************************************************************************************************/

#define HMC5883L_I2C_TIMEOUT     1000    // I2C超时计数

/*************************************************************************************************/
/* 私有变量 */
/*************************************************************************************************/

static HMC5883L_Config_t g_hmc5883l_config = {
    .samples = HMC5883L_SAMPLES_8,           // 默认每次测量8个样本
    .dataRate = HMC5883L_DATARATE_15,        // 默认输出速率15Hz
    .gain = HMC5883L_GAIN_1090,              // 默认增益±1.3Ga
    .measureMode = HMC5883L_MODE_CONTINUOUS, // 默认连续测量模式
    .highSpeed = true,                       // 默认高速I2C (400kHz)
    .declination = 0.0f,                     // 默认磁偏角0度(可根据地区调整)
    .scale = 1.0f/1090.0f                    // 默认比例因子(根据增益设置)
};

// 校准数据
static HMC5883L_CalibrationData_t g_cal_data = {
    .x_offset = 0, .y_offset = 0, .z_offset = 0,
    .x_scale = 1.0f, .y_scale = 1.0f, .z_scale = 1.0f,
    .initialized = false
};

// 校准过程中使用的临时变量
static int16_t g_cal_x_min, g_cal_x_max;
static int16_t g_cal_y_min, g_cal_y_max;
static int16_t g_cal_z_min, g_cal_z_max;

static volatile bool g_data_ready = false;         // 数据就绪标志
static volatile bool g_is_calibrating = false;     // 正在校准标志


// EMA滤波器状态变量
static float g_filtered_mag_x = 0.0f;
static float g_filtered_mag_y = 0.0f;
static float g_filtered_mag_z = 0.0f;
static float g_filter_alpha = HMC5883L_DEFAULT_FILTER_ALPHA;
static bool g_filter_initialized = false;


/*************************************************************************************************/
/* 私有函数声明 */
/*************************************************************************************************/

static uint8_t HMC5883L_WriteReg(uint8_t reg, uint8_t value);
static uint8_t HMC5883L_ReadReg(uint8_t reg, uint8_t *value);
static uint8_t HMC5883L_ReadMultiple(uint8_t reg, uint8_t *buffer, uint8_t length);
static float HMC5883L_ApplyCalibration(int16_t value, int16_t offset);
static void HMC5883L_UpdateCalibrationData(int16_t x, int16_t y, int16_t z);
static uint8_t HMC5883L_UpdateScale(void);
static uint8_t HMC5883L_VerifyDevice(void);
static uint8_t HMC5883L_I2C_Wait_Complete(void);

/*************************************************************************************************/
/* I2C基础函数实现 */
/*************************************************************************************************/

/**
 * @brief 等待I2C事务完成
 * @return HMC5883L_OK: 成功, HMC5883L_ERROR: 超时错误
 */
static uint8_t HMC5883L_I2C_Wait_Complete(void)
{
    uint32_t timeout = HMC5883L_I2C_TIMEOUT;
    
    while (DL_I2C_getControllerStatus(I2C_HMC5883L_INST) & DL_I2C_CONTROLLER_STATUS_BUSY_BUS) {
        timeout--;
        if (timeout == 0) {
            // 清除任何错误状态
            DL_I2C_flushControllerTXFIFO(I2C_HMC5883L_INST);
            DL_I2C_flushControllerRXFIFO(I2C_HMC5883L_INST);
            return HMC5883L_ERROR;  // 超时错误
        }
        // 小延迟避免忙等
        for(volatile int i = 0; i < 10; i++);
    }
    return HMC5883L_OK;  // 成功
}

/**
 * @brief 写单个寄存器
 * @param reg 寄存器地址
 * @param value 要写入的值
 * @return HMC5883L_OK: 写入成功, HMC5883L_ERROR: 写入失败
 */
static uint8_t HMC5883L_WriteReg(uint8_t reg, uint8_t value)
{
    uint8_t tx_data[2] = {reg, value};
    
    // 重置之前的事务
    DL_I2C_flushControllerTXFIFO(I2C_HMC5883L_INST);
    
    // 开始I2C事务
    DL_I2C_startControllerTransfer(I2C_HMC5883L_INST, HMC5883L_I2C_ADDR, DL_I2C_CONTROLLER_DIRECTION_TX, 2);
    
    // 发送寄存器地址和数据
    DL_I2C_fillControllerTXFIFO(I2C_HMC5883L_INST, tx_data, 2);
    
    // 等待事务完成
    if (HMC5883L_I2C_Wait_Complete() != HMC5883L_OK) {
        return HMC5883L_ERROR;  // 超时错误
    }
    
    return HMC5883L_OK;  // 成功
}

/**
 * @brief 读单个寄存器
 * @param reg 寄存器地址
 * @param value 存储读取值的指针
 * @return HMC5883L_OK: 读取成功, HMC5883L_ERROR: 读取失败
 */
static uint8_t HMC5883L_ReadReg(uint8_t reg, uint8_t *value)
{
    uint32_t timeout = HMC5883L_I2C_TIMEOUT;
    
    if (value == NULL) {
        return HMC5883L_ERROR;
    }
    
    // 重置之前的事务
    DL_I2C_flushControllerTXFIFO(I2C_HMC5883L_INST);
    DL_I2C_flushControllerRXFIFO(I2C_HMC5883L_INST);
    
    // 首先，写入寄存器地址
    DL_I2C_startControllerTransfer(I2C_HMC5883L_INST, HMC5883L_I2C_ADDR, DL_I2C_CONTROLLER_DIRECTION_TX, 1);
    DL_I2C_fillControllerTXFIFO(I2C_HMC5883L_INST, &reg, 1);
    
    // 等待写入完成
    if (HMC5883L_I2C_Wait_Complete() != HMC5883L_OK) {
        return HMC5883L_ERROR;  // 超时错误
    }
    
    // 然后，读取数据
    DL_I2C_startControllerTransfer(I2C_HMC5883L_INST, HMC5883L_I2C_ADDR, DL_I2C_CONTROLLER_DIRECTION_RX, 1);
    
    // 等待数据可用
    while (DL_I2C_isControllerRXFIFOEmpty(I2C_HMC5883L_INST)) {
        timeout--;
        if (timeout == 0) {
            return HMC5883L_ERROR; // 超时错误
        }
    }
    
    *value = DL_I2C_receiveControllerData(I2C_HMC5883L_INST);
    
    // 等待事务完成
    HMC5883L_I2C_Wait_Complete();
    
    return HMC5883L_OK;
}

/**
 * @brief 读多个寄存器
 * @param reg 起始寄存器地址
 * @param buffer 存储读取数据的缓冲区
 * @param length 要读取的字节数
 * @return HMC5883L_OK: 读取成功, HMC5883L_ERROR: 读取失败
 */
static uint8_t HMC5883L_ReadMultiple(uint8_t reg, uint8_t *buffer, uint8_t length)
{
    uint8_t i;
    
    if (buffer == NULL) {
        return HMC5883L_ERROR;
    }
    
    // 重置之前的事务
    DL_I2C_flushControllerTXFIFO(I2C_HMC5883L_INST);
    DL_I2C_flushControllerRXFIFO(I2C_HMC5883L_INST);
    
    // 首先，写入寄存器地址
    DL_I2C_startControllerTransfer(I2C_HMC5883L_INST, HMC5883L_I2C_ADDR, DL_I2C_CONTROLLER_DIRECTION_TX, 1);
    DL_I2C_fillControllerTXFIFO(I2C_HMC5883L_INST, &reg, 1);
    
    // 等待写入完成
    if (HMC5883L_I2C_Wait_Complete() != HMC5883L_OK) {
        return HMC5883L_ERROR;  // 超时错误
    }
    
    // 然后，读取数据
    DL_I2C_startControllerTransfer(I2C_HMC5883L_INST, HMC5883L_I2C_ADDR, DL_I2C_CONTROLLER_DIRECTION_RX, length);
    
    // 读取所有数据
    for (i = 0; i < length; i++) {
        // 等待数据可用
        uint32_t timeout = HMC5883L_I2C_TIMEOUT;
        while (DL_I2C_isControllerRXFIFOEmpty(I2C_HMC5883L_INST)) {
            timeout--;
            if (timeout == 0) {
                return HMC5883L_ERROR; // 超时错误
            }
        }
        buffer[i] = DL_I2C_receiveControllerData(I2C_HMC5883L_INST);
    }
    
    // 等待事务完成
    HMC5883L_I2C_Wait_Complete();
    
    return HMC5883L_OK;
}

/*************************************************************************************************/
/* 驱动功能实现 */
/*************************************************************************************************/

/**
 * @brief 初始化HMC5883L传感器
 * @return HMC5883L_OK: 初始化成功, HMC5883L_ERROR: 初始化失败
 */
uint8_t HMC5883L_Init(void)
{
    // 1. 验证设备存在
    if (HMC5883L_VerifyDevice() != HMC5883L_OK) {
        return HMC5883L_ERROR;
    }
    
    // 2. 软重置设备
    if (HMC5883L_Reset() != HMC5883L_OK) {
        return HMC5883L_ERROR;
    }
    
    // 3. 应用默认配置
    uint8_t config_a = g_hmc5883l_config.samples | g_hmc5883l_config.dataRate | HMC5883L_NORMAL_MODE;
    if (HMC5883L_WriteReg(HMC5883L_REG_CONFIG_A, config_a) != HMC5883L_OK) {
        return HMC5883L_ERROR;
    }
    
    // 4. 设置增益
    if (HMC5883L_SetGain(g_hmc5883l_config.gain) != HMC5883L_OK) {
        return HMC5883L_ERROR;
    }
    
    // 5. 设置测量模式
    if (HMC5883L_SetMode(g_hmc5883l_config.measureMode) != HMC5883L_OK) {
        return HMC5883L_ERROR;
    }
    
    // 6. 更新比例因子
    if (HMC5883L_UpdateScale() != HMC5883L_OK) {
        return HMC5883L_ERROR;
    }
    
    // 7. 等待第一个数据样本准备好
    mspm0_delay_ms(10);
    
    return HMC5883L_OK;
}

/**
 * @brief 验证HMC5883L设备是否存在
 * @return HMC5883L_OK: 设备存在, HMC5883L_ERROR: 设备不存在
 */
static uint8_t HMC5883L_VerifyDevice(void)
{
    uint8_t id[3];
    
    // 读取设备ID寄存器(A, B, C)
    if (HMC5883L_ReadMultiple(HMC5883L_REG_ID_A, id, 3) != HMC5883L_OK) {
        return HMC5883L_ERROR;
    }
    
    // 验证ID是否符合HMC5883L的标识 (通常为'H', '4', '3')
    if (id[0] != 'H' || id[1] != '4' || id[2] != '3') {
        return HMC5883L_ERROR;
    }
    
    return HMC5883L_OK;
}

/**
 * @brief 软重置HMC5883L
 * @return HMC5883L_OK: 重置成功, HMC5883L_ERROR: 重置失败
 */
uint8_t HMC5883L_Reset(void)
{
    // 设置配置寄存器A (设置正常测量模式)
    if (HMC5883L_WriteReg(HMC5883L_REG_CONFIG_A, 0x70) != HMC5883L_OK) {
        return HMC5883L_ERROR;
    }
    
    // 设置配置寄存器B (默认增益设置)
    if (HMC5883L_WriteReg(HMC5883L_REG_CONFIG_B, 0x20) != HMC5883L_OK) {
        return HMC5883L_ERROR;
    }
    
    // 设置模式寄存器 (连续测量模式)
    if (HMC5883L_WriteReg(HMC5883L_REG_MODE, 0x00) != HMC5883L_OK) {
        return HMC5883L_ERROR;
    }
    
    // 等待设备重置完成
    mspm0_delay_ms(10);
    
    return HMC5883L_OK;
}

/**
 * @brief 设置HMC5883L配置
 * @param config 配置结构体指针
 * @return HMC5883L_OK: 设置成功, HMC5883L_ERROR: 设置失败
 */
uint8_t HMC5883L_SetConfig(const HMC5883L_Config_t *config)
{
    if (config == NULL) {
        return HMC5883L_ERROR;
    }
    
    // 保存配置
    g_hmc5883l_config = *config;
    
    // 设置配置寄存器A
    uint8_t config_a = config->samples | config->dataRate | HMC5883L_NORMAL_MODE;
    if (HMC5883L_WriteReg(HMC5883L_REG_CONFIG_A, config_a) != HMC5883L_OK) {
        return HMC5883L_ERROR;
    }
    
    // 设置增益
    if (HMC5883L_SetGain(config->gain) != HMC5883L_OK) {
        return HMC5883L_ERROR;
    }
    
    // 设置测量模式
    if (HMC5883L_SetMode(config->measureMode) != HMC5883L_OK) {
        return HMC5883L_ERROR;
    }
    
    // 更新比例因子
    if (HMC5883L_UpdateScale() != HMC5883L_OK) {
        return HMC5883L_ERROR;
    }
    
    return HMC5883L_OK;
}

/**
 * @brief 获取HMC5883L当前配置
 * @param config 配置结构体指针(用于存储获取的配置)
 * @return HMC5883L_OK: 获取成功, HMC5883L_ERROR: 获取失败
 */
uint8_t HMC5883L_GetConfig(HMC5883L_Config_t *config)
{
    if (config == NULL) {
        return HMC5883L_ERROR;
    }
    
    // 复制当前配置
    *config = g_hmc5883l_config;
    
    return HMC5883L_OK;
}

/**
 * @brief 根据增益设置更新比例因子
 * @return HMC5883L_OK: 更新成功, HMC5883L_ERROR: 更新失败
 */
static uint8_t HMC5883L_UpdateScale(void)
{
    // 根据增益设置选择合适的比例因子
    switch (g_hmc5883l_config.gain) {
        case HMC5883L_GAIN_1370:
            g_hmc5883l_config.scale = 1.0f / 1370.0f;
            break;
        case HMC5883L_GAIN_1090:
            g_hmc5883l_config.scale = 1.0f / 1090.0f;
            break;
        case HMC5883L_GAIN_820:
            g_hmc5883l_config.scale = 1.0f / 820.0f;
            break;
        case HMC5883L_GAIN_660:
            g_hmc5883l_config.scale = 1.0f / 660.0f;
            break;
        case HMC5883L_GAIN_440:
            g_hmc5883l_config.scale = 1.0f / 440.0f;
            break;
        case HMC5883L_GAIN_390:
            g_hmc5883l_config.scale = 1.0f / 390.0f;
            break;
        case HMC5883L_GAIN_330:
            g_hmc5883l_config.scale = 1.0f / 330.0f;
            break;
        case HMC5883L_GAIN_230:
            g_hmc5883l_config.scale = 1.0f / 230.0f;
            break;
        default:
            // 无效增益设置
            return HMC5883L_ERROR;
    }
    
    return HMC5883L_OK;
}

/**
 * @brief 设置HMC5883L测量模式
 * @param mode 测量模式(HMC5883L_MODE_CONTINUOUS, HMC5883L_MODE_SINGLE, HMC5883L_MODE_IDLE)
 * @return HMC5883L_OK: 设置成功, HMC5883L_ERROR: 设置失败
 */
uint8_t HMC5883L_SetMode(uint8_t mode)
{
    // 验证模式值
    if (mode > HMC5883L_MODE_IDLE_2) {
        return HMC5883L_ERROR;
    }
    
    // 写入模式寄存器
    if (HMC5883L_WriteReg(HMC5883L_REG_MODE, mode) != HMC5883L_OK) {
        return HMC5883L_ERROR;
    }
    
    // 保存当前模式
    g_hmc5883l_config.measureMode = mode;
    
    return HMC5883L_OK;
}

/**
 * @brief 设置HMC5883L增益
 * @param gain 增益设置值(HMC5883L_GAIN_*)
 * @return HMC5883L_OK: 设置成功, HMC5883L_ERROR: 设置失败
 */
uint8_t HMC5883L_SetGain(uint8_t gain)
{
    // 验证增益值
    if ((gain & 0x1F) != 0) { // 检查非法位
        return HMC5883L_ERROR;
    }
    
    // 写入增益寄存器
    if (HMC5883L_WriteReg(HMC5883L_REG_CONFIG_B, gain) != HMC5883L_OK) {
        return HMC5883L_ERROR;
    }
    
    // 保存当前增益设置
    g_hmc5883l_config.gain = gain;
    
    // 更新比例因子
    return HMC5883L_UpdateScale();
}

/**
 * @brief 设置HMC5883L采样率
 * @param dataRate 采样率设置值(HMC5883L_DATARATE_*)
 * @return HMC5883L_OK: 设置成功, HMC5883L_ERROR: 设置失败
 */
uint8_t HMC5883L_SetDataRate(uint8_t dataRate)
{
    uint8_t value;
    
    // 读取当前配置寄存器A
    if (HMC5883L_ReadReg(HMC5883L_REG_CONFIG_A, &value) != HMC5883L_OK) {
        return HMC5883L_ERROR;
    }
    
    // 清除原数据速率位，设置新数据速率
    value &= ~(0x1C); // 清除位2-4
    value |= (dataRate & 0x1C); // 设置新数据速率
    
    // 写入更新后的配置
    if (HMC5883L_WriteReg(HMC5883L_REG_CONFIG_A, value) != HMC5883L_OK) {
        return HMC5883L_ERROR;
    }
    
    // 保存当前数据速率
    g_hmc5883l_config.dataRate = dataRate;
    
    return HMC5883L_OK;
}

/**
 * @brief 读取原始数据并转换为磁场强度
 * @param data 数据结构体指针(用于存储转换后的数据)
 * @return HMC5883L_OK: 转换成功, HMC5883L_ERROR: 转换失败
 */
uint8_t HMC5883L_ReadRawData(HMC5883L_Data_t *data)
{
    uint8_t buffer[6];
    
    if (data == NULL) {
        return HMC5883L_ERROR;
    }
    
    // 1. 从寄存器读取6个字节的数据 (X MSB, X LSB, Z MSB, Z LSB, Y MSB, Y LSB)
    if (HMC5883L_ReadMultiple(HMC5883L_REG_DATA_X_MSB, buffer, 6) != HMC5883L_OK) {
        return HMC5883L_ERROR;
    }
    
    // 2. 组合高低字节并转换为16位有符号整数
    data->x = ((int16_t)buffer[0] << 8) | buffer[1];
    data->z = ((int16_t)buffer[2] << 8) | buffer[3];
    data->y = ((int16_t)buffer[4] << 8) | buffer[5];
    
    // 3. 如果正在校准，更新最大/最小值
    if (g_is_calibrating) {
        HMC5883L_UpdateCalibrationData(data->x, data->y, data->z);
    }

    // 4. 应用校准参数（如果已初始化）
    if (g_cal_data.initialized) {
        data->mag_x = ((float)data->x - g_cal_data.x_offset) * g_cal_data.x_scale;
        data->mag_y = ((float)data->y - g_cal_data.y_offset) * g_cal_data.y_scale;
        data->mag_z = ((float)data->z - g_cal_data.z_offset) * g_cal_data.z_scale;
    } else {
        // 未校准，直接使用原始值
        data->mag_x = (float)data->x;
        data->mag_y = (float)data->y;
        data->mag_z = (float)data->z;
    }
    
    // 5. 应用增益比例因子，转换为高斯单位
    data->mag_x *= g_hmc5883l_config.scale;
    data->mag_y *= g_hmc5883l_config.scale;
    data->mag_z *= g_hmc5883l_config.scale;
    
    // 6. 记录时间戳
    data->timestamp = tick_ms;
    
    return HMC5883L_OK;
}

/**
 * @brief 读取HMC5883L磁场数据并计算磁航向角
 * @param data 数据结构体指针(用于存储读取的数据)
 * @return HMC5883L_OK: 读取成功, HMC5883L_ERROR: 读取失败
 */
uint8_t HMC5883L_ReadMag(HMC5883L_Data_t *data)
{
    // 读取原始数据
    if (HMC5883L_ReadRawData(data) != HMC5883L_OK) {
        return HMC5883L_ERROR;
    }
    
    // 计算磁航向角
    data->heading = HMC5883L_CalculateHeading(data->mag_x, data->mag_y);
    
    // 清除数据就绪标志
    g_data_ready = false;
    
    return HMC5883L_OK;
}

/**
 * @brief 读取HMC5883L磁场数据并进行滤波
 * @param data 数据结构体指针(用于存储滤波后的数据)
 * @return HMC5883L_OK: 读取成功, HMC5883L_ERROR: 读取失败
 */
uint8_t HMC5883L_ReadMagFiltered(HMC5883L_Data_t *data)
{
    HMC5883L_Data_t raw_data;

    // 1. 读取原始数据
    if (HMC5883L_ReadRawData(&raw_data) != HMC5883L_OK) {
        return HMC5883L_ERROR;
    }

    // 2. 初始化滤波器
    if (!g_filter_initialized) {
        g_filtered_mag_x = raw_data.mag_x;
        g_filtered_mag_y = raw_data.mag_y;
        g_filtered_mag_z = raw_data.mag_z;
        g_filter_initialized = true;
    }

    // 3. 应用EMA滤波器
    g_filtered_mag_x = (g_filter_alpha * raw_data.mag_x) + (1.0f - g_filter_alpha) * g_filtered_mag_x;
    g_filtered_mag_y = (g_filter_alpha * raw_data.mag_y) + (1.0f - g_filter_alpha) * g_filtered_mag_y;
    g_filtered_mag_z = (g_filter_alpha * raw_data.mag_z) + (1.0f - g_filter_alpha) * g_filtered_mag_z;

    // 4. 填充返回的数据结构
    memcpy(data, &raw_data, sizeof(HMC5883L_Data_t)); // 复制原始数据（如x,y,z,timestamp）
    data->mag_x = g_filtered_mag_x;
    data->mag_y = g_filtered_mag_y;
    data->mag_z = g_filtered_mag_z;

    // 5. 使用滤波后的数据计算航向角
    data->heading = HMC5883L_CalculateHeading(data->mag_x, data->mag_y);
    
    // 清除数据就绪标志
    g_data_ready = false;

    return HMC5883L_OK;
}

/**
 * @brief 设置滤波器的平滑因子 (alpha)
 * @param alpha 平滑因子 (0.0 to 1.0)
 */
void HMC5883L_SetFilterAlpha(float alpha)
{
    if (alpha >= 0.0f && alpha <= 1.0f) {
        g_filter_alpha = alpha;
    }
}

/**
 * @brief 计算磁航向角
 * @param x X轴磁场值
 * @param y Y轴磁场值
 * @return 磁航向角(度,0-360)
 */
float HMC5883L_CalculateHeading(float x, float y)
{
    float heading;
    
    // 计算航向角(弧度)
    // 注意坐标系转换，可能需要根据传感器安装方向调整
    heading = atan2f(y, x);
    
    // 应用磁偏角校正
    heading += g_hmc5883l_config.declination * (M_PI / 180.0f);
    
    // 规范化到0-2π范围
    if (heading < 0) {
        heading += 2 * M_PI;
    } else if (heading > 2 * M_PI) {
        heading -= 2 * M_PI;
    }
    
    // 转换为度数(0-360)
    heading = heading * 180.0f / M_PI;
    
    return heading;
}

/**
 * @brief 设置HMC5883L磁偏角
 * @param declination 磁偏角(度)
 */
void HMC5883L_SetDeclination(float declination)
{
    g_hmc5883l_config.declination = declination;
}

/**
 * @brief 获取HMC5883L磁偏角
 * @return 当前设置的磁偏角(度)
 */
float HMC5883L_GetDeclination(void)
{
    return g_hmc5883l_config.declination;
}

/**
 * @brief 应用校准参数
 * @param value 原始值
 * @param offset 偏移量
 * @return 校准后的值
 */
static float HMC5883L_ApplyCalibration(int16_t value, int16_t offset)
{
    return (float)(value - offset);
}

/**
 * @brief 更新校准过程中的最大/最小值
 * @param x X轴原始值
 * @param y Y轴原始值
 * @param z Z轴原始值
 */
static void HMC5883L_UpdateCalibrationData(int16_t x, int16_t y, int16_t z)
{
    // 更新X轴最大最小值
    if (x < g_cal_x_min) g_cal_x_min = x;
    if (x > g_cal_x_max) g_cal_x_max = x;
    
    // 更新Y轴最大最小值
    if (y < g_cal_y_min) g_cal_y_min = y;
    if (y > g_cal_y_max) g_cal_y_max = y;
    
    // 更新Z轴最大最小值
    if (z < g_cal_z_min) g_cal_z_min = z;
    if (z > g_cal_z_max) g_cal_z_max = z;
}

/**
 * @brief 启动HMC5883L硬铁和软铁校准过程
 */
void HMC5883L_Calibration_Start(void)
{
    g_is_calibrating = true;
    g_cal_data.initialized = false;
    
    // 重置最大/最小值
    g_cal_x_min = 32767; g_cal_x_max = -32768;
    g_cal_y_min = 32767; g_cal_y_max = -32768;
    g_cal_z_min = 32767; g_cal_z_max = -32768;
}

/**
 * @brief 停止HMC5883L校准过程并计算校准参数
 */
void HMC5883L_Calibration_Stop(void)
{
    if (!g_is_calibrating) {
        return; // 未在校准模式
    }
    
    g_is_calibrating = false;

    // 1. 计算硬铁偏移 (hard-iron offsets)
    g_cal_data.x_offset = (g_cal_x_max + g_cal_x_min) / 2;
    g_cal_data.y_offset = (g_cal_y_max + g_cal_y_min) / 2;
    g_cal_data.z_offset = (g_cal_z_max + g_cal_z_min) / 2;

    // 2. 计算软铁缩放比例 (soft-iron scaling)
    float x_range = (float)(g_cal_x_max - g_cal_x_min) / 2.0f;
    float y_range = (float)(g_cal_y_max - g_cal_y_min) / 2.0f;
    float z_range = (float)(g_cal_z_max - g_cal_z_min) / 2.0f;

    // 避免除以零
    if (x_range < 1.0f) x_range = 1.0f;
    if (y_range < 1.0f) y_range = 1.0f;
    if (z_range < 1.0f) z_range = 1.0f;

    // 计算平均半径
    float avg_range = (x_range + y_range + z_range) / 3.0f;

    // 计算缩放因子
    g_cal_data.x_scale = avg_range / x_range;
    g_cal_data.y_scale = avg_range / y_range;
    g_cal_data.z_scale = avg_range / z_range;
    
    g_cal_data.initialized = true;
}

/**
 * @brief 检查校准过程是否正在运行
 * @return true: 正在校准, false: 未在校准
 */
bool HMC5883L_Calibration_IsRunning(void)
{
    return g_is_calibrating;
}

/**
 * @brief 加载校准数据
 * @param cal_data 指向包含校准数据的结构体
 */
void HMC5883L_Calibration_Load(const HMC5883L_CalibrationData_t *cal_data)
{
    if (cal_data != NULL) {
        g_cal_data = *cal_data;
    }
}

/**
 * @brief 获取当前校准数据
 * @param cal_data 指向用于存储校准数据的结构体
 */
void HMC5883L_Calibration_Get(HMC5883L_CalibrationData_t *cal_data)
{
    if (cal_data != NULL) {
        *cal_data = g_cal_data;
    }
}


/**
 * @brief 执行HMC5883L自测
 * @return HMC5883L_OK: 自测通过, HMC5883L_ERROR: 自测失败
 */
uint8_t HMC5883L_SelfTest(void)
{
    uint8_t old_mode;
    uint8_t old_gain;
    uint8_t old_config_a;
    uint8_t status;
    
    // 保存当前配置
    old_mode = g_hmc5883l_config.measureMode;
    old_gain = g_hmc5883l_config.gain;
    old_config_a = g_hmc5883l_config.samples | g_hmc5883l_config.dataRate | HMC5883L_NORMAL_MODE;
    
    // 1. 设置自测配置（遵循手册）
    // CRA = 0x71: 8 samples (0x60), 15Hz (0x10), positive bias (0x01)
    if (HMC5883L_WriteReg(HMC5883L_REG_CONFIG_A, 0x71) != HMC5883L_OK) {
        return HMC5883L_ERROR;
    }
    
    // CRB = 0xA0: Gain setting 5 (390 LSB/Gauss, ±4.7 Ga)
    if (HMC5883L_WriteReg(HMC5883L_REG_CONFIG_B, HMC5883L_GAIN_390) != HMC5883L_OK) {
        return HMC5883L_ERROR;
    }
    
    // 2. 设置单次测量模式 (0x01)
    if (HMC5883L_SetMode(HMC5883L_MODE_SINGLE) != HMC5883L_OK) {
        return HMC5883L_ERROR;
    }
    
    // 等待测量完成（手册推荐6ms，或检查DRDY）
    mspm0_delay_ms(6);
    
    // 3. 读取正偏置模式的数据
    HMC5883L_Data_t pos_data;
    // 注意：这里我们只读取原始ADC值，不应用任何校准
    uint8_t buffer[6];
    if (HMC5883L_ReadMultiple(HMC5883L_REG_DATA_X_MSB, buffer, 6) != HMC5883L_OK) {
        return HMC5883L_ERROR;
    }
    pos_data.x = ((int16_t)buffer[0] << 8) | buffer[1];
    pos_data.z = ((int16_t)buffer[2] << 8) | buffer[3];
    pos_data.y = ((int16_t)buffer[4] << 8) | buffer[5];

    // 4. 判断自测是否通过（手册范围: 243-575 LSB for gain 5）
    bool test_pass = (pos_data.x >= 243 && pos_data.x <= 575) &&
                     (pos_data.y >= 243 && pos_data.y <= 575) &&
                     (pos_data.z >= 243 && pos_data.z <= 575);
    
    // 5. 恢复原始配置
    if (HMC5883L_WriteReg(HMC5883L_REG_CONFIG_A, old_config_a) != HMC5883L_OK) {
        return HMC5883L_ERROR;
    }
    if (HMC5883L_SetGain(old_gain) != HMC5883L_OK) {
        return HMC5883L_ERROR;
    }
    status = HMC5883L_SetMode(old_mode);
    
    // 等待恢复
    mspm0_delay_ms(10);
    
    return (test_pass && status == HMC5883L_OK) ? HMC5883L_OK : HMC5883L_ERROR;
}

/**
 * @brief 获取HMC5883L器件ID
 * @param id 存储ID的指针(3字节)
 * @return HMC5883L_OK: 获取成功, HMC5883L_ERROR: 获取失败
 */
uint8_t HMC5883L_GetID(uint8_t *id)
{
    if (id == NULL) {
        return HMC5883L_ERROR;
    }
    
    // 读取识别寄存器A, B, C
    return HMC5883L_ReadMultiple(HMC5883L_REG_ID_A, id, 3);
}

/**
 * @brief 数据就绪(DRDY)中断处理函数
 */
void HMC5883L_DRDY_Handler(void)
{
    // 设置数据就绪标志
    g_data_ready = true;
}

/**
 * @brief 判断HMC5883L是否数据就绪
 * @return true: 数据就绪, false: 数据未就绪
 */
bool HMC5883L_IsDataReady(void)
{
    // 这里我们只返回内部标志状态，不直接读取GPIO
    // 如果需要使用GPIO检测，可在中断处理函数中设置此标志
    return g_data_ready;
}
